/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

//////////////////////////////////////////////////////
// File: MatDemo1.cpp
//
// Tests the functionality of the matrix routines
//
                          
#include <stdlib.h>                    
#include <stdio.h>                    
#include <time.h>                    
#include "matrix.h"  
#include "utilities.h"  

void WaitForEnterKey ();

int main ()
{

	// Test coordinate conversions
	double lla_d[] = {40.0000, -105.25, 1770.0};
	Matrix lla(3,1,lla_d);

	Matrix ecef = lla2ecef(lla);
	cout << "LLA: " << endl << lla << endl;
	cout << "ECEF: " << endl << ecef << endl;
	Matrix lla1 = ecef2lla(ecef);
	cout << "LLA: " << endl << lla1 << endl;
	cout << "DELta: " << endl << lla1-lla << endl;

	Matrix dcm = ecef2nedDCM(lla);
	Matrix eye(3,3);
	eye.Identity();
	cout << "DCM: " << endl << dcm << endl;
	cout << "DCM * ~DCM: " << endl << dcm * ~dcm << endl;
	cout << "DCM * ~DCM - I: " << endl << dcm * ~dcm - eye << endl;

	cout << "Creating two random number matrices M1 and M2:\n";
	Matrix m1(4,4);
	Matrix m2(4,4);
	unsigned int i,j;

	srand( (unsigned)time( NULL ) );
	for (i=0; i < 4; i++)
		for (j=0; j < 4; j++)
			m1(i,j) = 12/((rand()%25)+1.0);
	m1.SetSize(4,4);


	for (i=0; i < 4; i++)
		for (j=0; j < 4; j++)
			m2(i,j) = 12/((rand()%25)+1.0);

	cout << "\nThe matrix M1 is:\n" << m1 << endl;
	WaitForEnterKey();

	cout << "\nThe matrix M2 is:\n" << m2 << endl;
	WaitForEnterKey();

	double *row1 = new double[ m2.NumRow() ];
	m2.getRow(1,row1);

	cout << "\nThe matrix M2.getRow(1) is:\n";
	for(i=0; i < m2.NumCol(); i++)
		cout << "row1[" << i << "]: " << row1[i] << endl;
	WaitForEnterKey();

	cout << "\nM1.Col(2) = M2.Row(1)\n";
	m1.setCol(2, row1);
	cout << "\nThe matrix M1 is:\n" << m1 << endl;
	WaitForEnterKey();
	delete [] row1;

	cout << "\nMatrix equality: M1 == M2\n\t" << 
		(m1 == m2 ? "true" : "false" ) << endl << endl;
	WaitForEnterKey();

	cout << "\nMatrix enequality: M1 != M2\n\t" << 
		(m1 != m2 ? "true" : "false" ) << endl << endl;
	WaitForEnterKey();

	Matrix m3 = m1 + m2;
	cout << "\nMatrix addition:\nM1 + M2 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = m1 - m2;
	cout << "\nMatrix subtraction:\nM1 - M2 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = m1 * m2;
	cout << "\nMatrix multiplication:\nM1 * M2 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = m1 * 1.5;
	cout << "\nMatrix scalar multiplication:\nM1 * 1.5 = \n" 
		<< m1 * 1.5 << endl
		<< "1.5 * M1\n" << 1.5 * m1 << endl;
	WaitForEnterKey();

	m3 = m1 / 1.5E-10;
	cout << "\nMatrix scalar division:\nM1 / 1.5E-10 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = !m1;
	cout << "\nMatrix inversion:\n!M1 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = !((~m1) * m1) * (~m1);
	cout << "\nPsuedo inversion:\n!M1 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = m1 ^ 4U;
	cout << "\nMatrix power:\nM ^ 4 = \n" << m3 << endl;
	WaitForEnterKey();

	m3 = ~m1;
	cout << "\nTranspose of M1:\n~M1 = \n" <<  m3 << endl;
	WaitForEnterKey();

	m3 = m1.Adj();
	cout << "\nAdjoint of M1:\nAdj M1 = \n" <<  m3 << endl;
	WaitForEnterKey();

	cout << "\nSaving M1 to file m1.mat";
	if( !m1.saveToFile("m1.mat") )
		 cerr << "Didn't save matrix\n";
	cout << "\nReading M1 from file m1.mat";
	Matrix m4("m1.mat");
	//if( !m3.fillFromFile("m1.mat") )
		 //cerr << "Didn't fill matrix\n";
	cout << "\nComparing read matrix from file";
	cout << "\n\t M1 == M4 : \t" <<
          (m1 == m4 ? "true" : "false" ) << endl;
	if( m1 != m4)
		cout << m1 << endl << m4 << endl << "m1-m4=" << m1-m4 << endl;
     WaitForEnterKey();

	cout << "\nDeterminant of M1 = " << m1.Det() << endl;
	cout << "Norm of M1 = " << m1.Norm() << endl;
	cout << "Cofactor of M1(0,0) = " << m1.Cofact(0,0) << endl;
	cout << "Condition No. of M1 = " << m1.Cond() << endl << endl;
	WaitForEnterKey();

	m3.SetSize(3,3);
	cout << "Enter a matrix of (3X3) order:\n";
	cin >> m3;
	cout << "This is a ";
		 if (m3.IsSquare())
			    cout << "Square ";
		 if (m3.IsSingular())
			    cout << "Singular ";
		 if (m3.IsDiagonal())
			    cout << "Diagonal ";
		 if (m3.IsUnit())
			    cout << "Unit ";
		 if (m3.IsNull())
			    cout << "Null ";
		 if (m3.IsSymmetric())
			    cout << "Symmetric ";
		 if (m3.IsSkewSymmetric())
			    cout << "SkewSymmetric ";
		 if (m3.IsUpperTriangular())
			    cout << "UpperTiangular ";
		 if (m3.IsLowerTriangular())
			    cout << "LowerTiangular ";
		 if (m3.IsScalar())
			    cout << "Scaler ";
	cout << "matrix" << endl;

	double t[] = {1,2,3,4,5,6};
	Matrix rowVec(1,5,t);
	Matrix colVec(5,1,t);
	Matrix mat(3,2,t);

	cout <<"Filled Matrix : \n" << mat << endl;

	cout <<"Row Vector: \n" << rowVec << endl;
	for(int i=0; i<5; i++)
		cout << "rowVec(" << i << ")=" << rowVec(i) << endl;

	cout <<"\nCol Vector: \n" << colVec << endl;
	for(int i=0; i<5; i++)
		cout << "colVec(" << i << ")=" << colVec(i) << endl;

	double t1[9] = {0, 0, -3, 0, 2, 0, -1, 0, 0};
	Matrix T1(3,3,t1);
	cout << "T1:\n" << T1 << endl;

	return 0;
}


void WaitForEnterKey ()
{
	char ch = '\0';

	cout << "Press ENTER to continue . . . ";
	cout.flush();

	while (!(ch == '\n' || ch == '\r'))
		ch = (char)getchar();

	return;
}


